#Created by 张荣开 on 25-6-1.

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64  # 导入正确的消息类型
import math


class ImuTargetPubNode(Node):
    def __init__(self):
        super().__init__('imu_target_pub_node')
        
        # 阿克曼前后轮距离
        self.WHEELBASE = 0.8
        # 固定线速度 (m/s)
        self.linear_velocity = 0.0
        
        # 创建速度发布者
        self.angle_pub = self.create_publisher(Float64, 'setpoint', 10)
        
        # 目标偏航角 (初始化为0)
        self.target_yaw = 0.0
        self.counter = 0
        # 创建定时器，定期发布控制指令
        self.timer = self.create_timer(1, self.timer_callback)
        
        self.get_logger().info('IMU阿克曼控制器已启动')

    # def angz_to_angle(self, Vx, Vz):
    #     """计算转向角"""
    #     if Vx == 0 or Vz == 0:
    #         return 0.0
    #     R = Vx / Vz
    #     return math.atan(self.WHEELBASE / R)  # 使用实例属性 self.WHEELBASE

    def set_target_yaw(self, yaw):
        self.get_logger().info(f'目标偏航角已设置为: {yaw} 弧度')
        
        # 计算转向角
        #angularz = self.angz_to_angle(self.linear_velocity, self.target_yaw)
        
        # 创建并发布消息
        angle_msg = Float64()
        angle_msg.data = yaw  # 正确设置消息的 data 字段
        self.angle_pub.publish(angle_msg)

    def timer_callback(self):
        """定时器回调：定期发布当前目标偏航角对应的控制指令"""
        self.set_target_yaw(self.target_yaw)
        self.counter += 1
        
        # 每print_interval次回调打印一次信息
        if self.counter >= 10:
            self.get_logger().info(f"当前目标偏航角: {self.target_yaw} 弧度")
            self.counter = 0  # 重置计数器

def main(args=None):
    rclpy.init(args=args)
    imu_target_pub_node = ImuTargetPubNode()
    rclpy.spin(imu_target_pub_node)
    imu_target_pub_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
